#include <Arduino.h>
#include <Servo.h> 
#include <avr/io.h>
#include <avr/interrupt.h>
#include "config.h"
#include "motors.h"
#include "ADC.h"
#include "debug.h"
#include "Timer.h"
#include "setup.h"

//UART
char data; // this var is used for reading data from uart


void setup_debug()
{
  robotInit();  
  forward(50);
  delay(1000);
  stopp();
  delay(1000);
  back (50);
  delay(1000);
  stopp();
}

void loop_debug()
{
  
 // forward(150);
  
  //Serial.print("analog right:");
 // Serial.print (  adc_data[DIST_SENSOR_CENTER] );
 // Serial.print(" center:");
 // Serial.println( adc_filtered_data[DIST_SENSOR_CENTER] );
/* 
      Serial.println("SENSOR INFO");
      Serial.print( leftTSOP );
      Serial.print(" ");
      Serial.print( centerTSOP );
      Serial.print(" ");
      Serial.println( rightTSOP );
      Serial.print("analog right:");
      Serial.print(  read_gp2y0a_range(adc_data[DIST_SENSOR_RIGHT]) );
      Serial.print(" center:");
      Serial.print( read_gp2y0a_range(adc_data[DIST_SENSOR_CENTER]) );
      Serial.print(" left:");
      Serial.println( read_gp2y0a_range(adc_data[DIST_SENSOR_LEFT]) );
      Serial.print("contact left: ");
      Serial.print(digitalRead(CONTACT_SENSOR_LEFT));
      Serial.print(" center: ");
      Serial.print(digitalRead(CONTACT_SENSOR_CENTER));
      Serial.print(" right:");
      Serial.println(digitalRead(CONTACT_SENSOR_RIGHT));
      delay(300);
  
  
  */
  
  
  
  if(Serial1.available())
  {
    data = Serial1.read(); 
    Serial1.print(data);
    switch(data)
    {
    case 'p':
      forward(180); 
      break;
    case 's':
      stopp(); 
      break;
    case 'l':
      back(110);
      break;
    case 'm':
      stopp(); 
      break;
    case 'a':
      writeServo(30); 
      break;
    case 'd':
      writeServo(-30); 
      break;
    case 'w':
      writeServo(0); 
      break;
    case 'h':
      Serial.println("SENSOR INFO");
      Serial.print( leftTSOP );
      Serial.print(" ");
      Serial.print( centerTSOP );
      Serial.print(" ");
      Serial.println( rightTSOP );
      Serial.print("analog right:");
      Serial.print (  read_gp2d12_range(adc_data[DIST_SENSOR_RIGHT]) );
      Serial.print(" center:");
      Serial.println( read_gp2y0a_range(adc_data[DIST_SENSOR_CENTER]) );
      Serial.print(" right:");
      Serial.println( read_gp2y0a_range(adc_data[DIST_SENSOR_LEFT]) );
      Serial.print("contact left: ");
      Serial.print(digitalRead(CONTACT_SENSOR_LEFT));
      Serial.print(" right:");
      Serial.println(digitalRead(CONTACT_SENSOR_RIGHT));
      delay(300);
       
    }
    delay(1);
    
  }
  
}

